#include <iostream>
#include <fstream>
#include <cstdlib>
#include "mcnpmit.hpp"
#include "moab/CartVect.hpp"
#include <cmath>

moab::Interface* mb_instance();

// Parameters
// const double pi   = 3.141592653589793;
const double c2pi = 0.1591549430918954;
// const double cpi  = 0.3183098861837907;

MCNPError next_number( std::string, double&, int& );
int how_many_numbers( std::string );
MCNPError read_numbers( std::string, int, std::vector< double >& );

// Constructor
McnpData::McnpData()
{

    // Default value for coordinate system
    coord_system = 0;

    // Default rotation matrix is identity matrix
    for( int i = 0; i < 4; i++ )
    {
        for( int j = 0; j < 4; j++ )
        {
            if( i == j )
                rotation_matrix[4 * i + j] = 1;
            else
                rotation_matrix[4 * i + j] = 0;
        }
    }
}

// Destructor
McnpData::~McnpData()
{

    // Vertices and elements
    MCNP_vertices.clear();
}

// Setting and retrieving coordinate sysem
MCNPError McnpData::set_coord_system( int k )
{
    coord_system = k;
    return MCNP_SUCCESS;
}
int McnpData::get_coord_system()
{
    return coord_system;
}

// Setting and retrieving roation matrix
MCNPError McnpData::set_rotation_matrix( double r[16] )
{
    for( int i = 0; i < 16; i++ )
    {
        rotation_matrix[i] = r[i];
    }
    return MCNP_SUCCESS;
}
double* McnpData::get_rotation_matrix()
{
    return rotation_matrix;
}

// Set the filename
MCNPError McnpData::set_filename( std::string fname )
{
    MCNP_filename = fname;
    return MCNP_SUCCESS;
}
std::string McnpData::get_filename()
{
    return MCNP_filename;
}

// Reading the MCNP file
MCNPError McnpData::read_mcnpfile( bool skip_mesh )
{

    MCNPError result;
    moab::ErrorCode MBresult;
    moab::CartVect tvect;

    std::vector< double > xvec[3];

    // Open the file
    std::ifstream mcnpfile;
    mcnpfile.open( MCNP_filename.c_str() );
    if( !mcnpfile )
    {
        std::cout << "Unable to open MCNP data file." << std::endl;
        return MCNP_FAILURE;
    }
    std::cout << std::endl;
    std::cout << "Reading MCNP input file..." << std::endl;

    // Prepare for file reading ...
    char line[10000];
    int mode = 0;  // Set the file reading mode to read proper data
    int nv[3];

    // Read in the file ...
    while( !mcnpfile.eof() )
    {

        mcnpfile.getline( line, 10000 );
        // std::cout << line << std::endl;

        switch( mode )
        {
            case 0:  // First line is a title
                mode++;
                break;
            case 1:  // Coordinate system
                mode++;
                result = read_coord_system( line );
                if( result == MCNP_FAILURE ) return MCNP_FAILURE;
                break;
            case 2:  // Rotation matrix
                mode++;
                for( int i = 0; i < 4; i++ )
                {
                    mcnpfile.getline( line, 10000 );
                    result = read_rotation_matrix( line, i );
                    if( result == MCNP_FAILURE ) return MCNP_FAILURE;
                }
                if( skip_mesh ) return MCNP_SUCCESS;
                break;
            case 3:  // Read in vertices and build elements
                mode++;

                for( int i = 0; i < 3; i++ )
                {
                    // How many points in the x[i]-direction
                    nv[i] = how_many_numbers( line );
                    if( nv[i] <= 0 ) return MCNP_FAILURE;

                    // Get space and read in these points
                    result = read_numbers( line, nv[i], xvec[i] );
                    if( result == MCNP_FAILURE ) return MCNP_FAILURE;

                    // Update to the next line
                    mcnpfile.getline( line, 10000 );
                }

                // Make the elements and vertices
                result = make_elements( xvec, nv );
                if( result == MCNP_FAILURE ) return MCNP_FAILURE;
                break;
            case 4:  // Read in tally data, make, and tag elements
                mode++;
                moab::EntityHandle elemhandle;

                moab::EntityHandle vstart, vijk;
                moab::EntityHandle connect[8];
                // double d[3];

                // vstart = MCNP_vertices.front();
                vstart = *( vert_handles.begin() );

                for( int i = 0; i < nv[0] - 1; i++ )
                {
                    for( int j = 0; j < nv[1] - 1; j++ )
                    {
                        for( int k = 0; k < nv[2] - 1; k++ )
                        {
                            vijk = vstart + ( i + j * nv[0] + k * nv[0] * nv[1] );

                            // std::cout << vijk << std::endl;

                            connect[0] = vijk;
                            connect[1] = vijk + 1;
                            connect[2] = vijk + 1 + nv[0];
                            connect[3] = vijk + nv[0];
                            connect[4] = vijk + nv[0] * nv[1];
                            connect[5] = vijk + 1 + nv[0] * nv[1];
                            connect[6] = vijk + 1 + nv[0] + nv[0] * nv[1];
                            connect[7] = vijk + nv[0] + nv[0] * nv[1];

                            MBresult = MBI->create_element( moab::MBHEX, connect, 8, elemhandle );
                            if( MBresult != moab::MB_SUCCESS ) return MCNP_FAILURE;
                            elem_handles.insert( elemhandle );

                            mcnpfile.getline( line, 10000 );
                            result = extract_tally_data( line, elemhandle );
                            if( result == MCNP_FAILURE ) return MCNP_FAILURE;
                        }
                    }
                }

                /*
                                  for (MBRange::iterator rit=vert_handles.begin(); rit !=
                   vert_handles.end(); ++rit) { std::cout << *rit << std::endl;
                                  }


                                  for (int i=0; i < nv[0]-1; i++) {
                                    for (int j=0; j < nv[1]-1; j++) {
                                      for (int k=0; k < nv[2]-1; k++) {
                                        vijk = vstart + (i + j*nv[0] + k*nv[0]*nv[1]);
                                        connect[0] = vijk;
                                        connect[1] = vijk + 1;
                                        connect[2] = vijk + 1 + nv[0];
                                        connect[3] = vijk + nv[0];
                                        connect[4] = vijk + nv[0]*nv[1];
                                        connect[5] = vijk + 1 + nv[0]*nv[1];
                                        connect[6] = vijk + 1 + nv[0] + nv[0]*nv[1];
                                        connect[7] = vijk + nv[0] + nv[0]*nv[1];

                                        MBresult = MBI->create_element(MBHEX, connect, 8,
                   elemhandle); if (MBresult != MB_SUCCESS) return MCNP_FAILURE;
                                        elem_handles.insert(elemhandle);

                                        mcnpfile.getline(line, 10000);
                                        result = extract_tally_data(line, elemhandle);
                                        if (result == MCNP_FAILURE) return MCNP_FAILURE;

                                    }
                                  }
                                }
                */
                break;
            case 5:  // Ckeck for weirdness at end of file
                if( !mcnpfile.eof() ) return MCNP_FAILURE;
                break;
        }
    }

    std::cout << "SUCCESS! Read in " << elem_handles.size() << " elements!" << std::endl << std::endl;
    // MCNP_vertices.clear();
    vert_handles.clear();
    MCNP_elems.clear();
    return MCNP_SUCCESS;
}

MCNPError McnpData::read_coord_system( std::string s )
{

    if( ( s.find( "Box" ) < 100 ) || ( s.find( "xyz" ) < 100 ) )
        coord_system = CARTESIAN;
    else if( s.find( "Cyl" ) < 100 )
        coord_system = CYLINDRICAL;
    else if( s.find( "Sph" ) < 100 )
        coord_system = SPHERICAL;
    else
        return MCNP_FAILURE;

    return MCNP_SUCCESS;
}

MCNPError McnpData::read_rotation_matrix( std::string s, int i )
{

    int fpos = 0;
    MCNPError result;

    for( int j = 0; j < 4; j++ )
    {
        result = next_number( s, rotation_matrix[4 * i + j], fpos );
        if( result == MCNP_FAILURE ) return MCNP_FAILURE;
    }

    return MCNP_SUCCESS;
}

MCNPError McnpData::make_elements( std::vector< double > x[3], int* n )
{

    // double v[3];
    // MBEntityHandle dumhandle;
    // MBEntityHandle vstart, vijk;
    unsigned int num_verts = n[0] * n[1] * n[2];
    double* coords;
    coords = new double[3 * num_verts];

    /*
          // Enter the vertices ...
          for (int k=0; k < n[2]; k++) {
                v[2] = x[2].at(k);
                for (int j=0; j < n[1]; j++) {
                      v[1] = x[1].at(j);
                      for (int i=0; i < n[0]; i++) {
                            v[0] = x[0].at(i);
                            MBresult = MBI->create_vertex(v, dumhandle);
                            if (MBresult != MB_SUCCESS) return MCNP_FAILURE;
                            MCNP_vertices.push_back(dumhandle);

                      }
                }
          }
    */

    // Enter the vertices ...
    for( int k = 0; k < n[2]; k++ )
    {
        for( int j = 0; j < n[1]; j++ )
        {
            for( int i = 0; i < n[0]; i++ )
            {
                unsigned int ijk = 3 * ( k * n[0] * n[1] + j * n[0] + i );
                coords[ijk]      = x[0][i];
                coords[ijk + 1]  = x[1][j];
                coords[ijk + 2]  = x[2][k];

                // std::cout << coords[ijk] << " " << coords[ijk+1] << " "
                //          << coords[ijk+2] << std::endl;

                // MCNP_vertices.push_back(dumhandle);
            }
        }
    }

    MBI->create_vertices( coords, num_verts, vert_handles );

    delete[] coords;
    return MCNP_SUCCESS;
}

MCNPError McnpData::initialize_tags()
{

    MBI->tag_get_handle( TALLY_TAG, 1, moab::MB_TYPE_DOUBLE, tally_tag, moab::MB_TAG_DENSE | moab::MB_TAG_CREAT );
    MBI->tag_get_handle( ERROR_TAG, 1, moab::MB_TYPE_DOUBLE, relerr_tag, moab::MB_TAG_DENSE | moab::MB_TAG_CREAT );

    return MCNP_SUCCESS;
}

MCNPError McnpData::extract_tally_data( std::string s, moab::EntityHandle handle )
{

    int fpos = 0;
    double d = 0;

    MCNPError result;
    moab::ErrorCode MBresult;

    // Discard first three lines
    for( int i = 0; i < 3; i++ )
    {
        result = next_number( s, d, fpos );
        if( result == MCNP_FAILURE ) return MCNP_FAILURE;
    }
    // Need to read in tally entry and tag ...
    result = next_number( s, d, fpos );
    if( result == MCNP_FAILURE ) return MCNP_FAILURE;
    MBresult = MBI->tag_set_data( tally_tag, &handle, 1, &d );
    if( MBresult != moab::MB_SUCCESS ) return MCNP_FAILURE;

    // Need to read in relative error entry and tag ...
    result = next_number( s, d, fpos );
    if( result == MCNP_FAILURE ) return MCNP_FAILURE;
    MBresult = MBI->tag_set_data( relerr_tag, &handle, 1, &d );
    if( MBresult != moab::MB_SUCCESS ) return MCNP_FAILURE;

    return MCNP_SUCCESS;
}

MCNPError next_number( std::string s, double& d, int& p )
{

    unsigned int slen = s.length();
    unsigned int j;

    for( unsigned int i = p; i < slen; i++ )
    {
        if( ( ( s[i] >= 48 ) && ( s[i] <= 57 ) ) || ( s[i] == 45 ) )
        {

            j = s.find( " ", i );

            if( j > slen ) j = slen;

            // Extract the number out of the string
            d = std::atof( s.substr( i, j - i ).c_str() );
            p = j + 1;

            return MCNP_SUCCESS;
        }
    }

    return DONE;
}

int how_many_numbers( std::string s )
{

    int n            = -1;
    int fpos         = 0;
    double d         = 0;
    MCNPError result = MCNP_SUCCESS;

    while( result != DONE )
    {
        result = next_number( s, d, fpos );
        n++;
    }

    return n;
}

MCNPError read_numbers( std::string s, int n, std::vector< double >& x )
{

    MCNPError result;
    int fpos = 0;
    double d;

    for( int i = 0; i < n; i++ )
    {
        result = next_number( s, d, fpos );
        if( result == MCNP_FAILURE ) return MCNP_FAILURE;
        x.push_back( d );
    }

    return MCNP_SUCCESS;
}

MCNPError McnpData::transform_point( double* p, double* r, int csys, double* rmat )
{

    double q[3];

    // Apply the rotation matrix
    for( unsigned int i = 0; i < 3; i++ )
    {
        q[i] = p[0] * rmat[4 * i] + p[1] * rmat[4 * i + 1] + p[2] * rmat[4 * i + 2] + rmat[4 * i + 3];
    }

    // Transform coordinate system
    switch( csys )
    {
        case CARTESIAN:
            r[0] = q[0];
            r[1] = q[1];
            r[2] = q[2];  // x, y, z
            break;
        case CYLINDRICAL:
            r[0] = sqrt( q[0] * q[0] + q[1] * q[1] );  // r
            r[1] = q[2];                               // z
            r[2] = c2pi * ( atan2( q[1], q[0] ) );     // theta (in rotations)
            break;
        case SPHERICAL:
            return MCNP_FAILURE;
        // break;
        default:
            return MCNP_FAILURE;
            // break;
    }

    return MCNP_SUCCESS;
}
